/**
平衡车驱动程序，注意loop循环的执行速度越快，效果越好，我之前就出现了因为过多的Serial打印输出，导致循环执行速度过慢，电机出现震荡的现象，所以不要把过多的程序放在loop循环当中可以使用FreeRTOS来解决，或者使用定时器
*/
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif

#define LED_BUILTIN 2


#include <Arduino.h>
#include <../.pio/libdeps/esp32dev/Simple FOC/src/SimpleFOC.h>
#include <../.pio/libdeps/esp32dev/MPU6050_tockn/src/MPU6050_tockn.h>
#include <../.pio/libdeps/esp32dev/XboxSeriesXControllerESP32_asukiaaa/src/XboxSeriesXControllerESP32_asukiaaa.hpp>
#include <../.pio/libdeps/esp32dev/XboxControllerNotificationParser/src/XboxControllerNotificationParser.h>
#include "../.pio/libdeps/esp32dev/FastLED/src/FastLED.h"  // 此示例程序需要使用FastLED库

float pid_stb_P = -1.55;
float pid_stb_I = -0.74;
float pid_stb_D = 0;
float pid_vel_P = -6.0;
float pid_vel_I = 0;
float pid_vel_D = -1.4;
float pid_steer_P = 0.8;
float pid_steer_I = 0;
float pid_steer_D = 0;
float lpf_pitch_value = 0.32;
float lpf_throttle_value = 0.2;
float lpf_steering_value = 0.1;
float Offset_parameters = 0.135; //偏置参数
//电机实例
BLDCMotor motor0 = BLDCMotor(7);
BLDCDriver3PWM driver0 = BLDCDriver3PWM(12, 14, 27, 13);//M0电机

BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(4, 0, 16, 13);//M1电机

//编码器实例
MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
TwoWire I2C_one = TwoWire(0);
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
TwoWire I2C_two = TwoWire(1);

// 在线电流检测实例
InlineCurrentSense current_sense0 = InlineCurrentSense(0.001, 264.0, 32, 33);
InlineCurrentSense current_sense1 = InlineCurrentSense(0.001, 264.0, 25, 26);
// control algorithm parameters
// stabilisation pid 自稳PID
//P0.55 I5.5 初始值0.5 5
PIDController pid_stb{.P = pid_stb_P, .I = pid_stb_I, .D = pid_stb_D, .ramp = 100000, .limit = 8};
// velocity pid 速度PID P初始值1.5
PIDController pid_vel{.P = pid_vel_P, .I = pid_vel_I, .D = pid_vel_D, .ramp = 100000, .limit = 4};
//steering PID用于克服轮子有点点问题的时候，没办法完全直行，使得直行的时候两个轮子的速度一致
PIDController pid_right_steer{.P = pid_steer_P, .I = pid_steer_I, .D = pid_steer_D, .ramp = 100000, .limit = 5};
PIDController pid_left_steer{.P = pid_steer_P, .I = pid_steer_I, .D = pid_steer_D, .ramp = 100000, .limit = 5};
// velocity control filtering 速度控制低通滤波，滤波时间常数为0.07
LowPassFilter lpf_pitch_cmd{.Tf = lpf_pitch_value};
// low pass filters for user commands - throttle and steering 油门和转向滤波
LowPassFilter lpf_throttle{.Tf = lpf_throttle_value}; //初始值0.5
LowPassFilter lpf_steering{.Tf = lpf_steering_value};//初始值0.1
// commander通信实例
Commander commander = Commander(Serial);
float steering = 0;
float throttle = 0;
// max_throttle数值可能需要更改
float max_throttle = 20; // 初始值20 rad/s
float max_steering = 0.8; //  1V
//借助Simplefoc自带的Commander类来通过串口快速调节和设置自己的PID和低通滤波参数
void cntStab(char *cmd) { commander.pid(&pid_stb, cmd); }

void cntMove(char *cmd) { commander.pid(&pid_vel, cmd); }

void lpfPitch(char *cmd) { commander.lpf(&lpf_pitch_cmd, cmd); }

void lpfSteering(char *cmd) { commander.lpf(&lpf_steering, cmd); }

void lpfThrottle(char *cmd) { commander.lpf(&lpf_throttle, cmd); }

void doMotor0(char *cmd) { commander.motor(&motor0, cmd); }

void doMotor1(char *cmd) { commander.motor(&motor1, cmd); }

MPU6050 mpu6050(I2C_one);//mpu6050使用I2C_one的I2C引脚

////////////////////////////蜂鸣器响函数////////////////////////////
#define BUZZER (15)
int BUZZER_channel = 1;//通道号，取值0 ~ 15
int frequency_list[][3] = {500, 1500, 2500, 2500, 1500, 500};

void BUZZER_setup() {
    int BUZZER_freq = 2500;//设置频率
    int BUZZER_resolution = 8;//分辨率，2的8次幂=256
    ledcSetup(BUZZER_channel, BUZZER_freq, BUZZER_resolution);
    ledcAttachPin(BUZZER, BUZZER_channel);
}

/// 无源蜂鸣器响铃函数
/// \param beep_times 需要蜂鸣器响的次数，小于三次
/// \param Tone_Mode 响铃模式，目前只有两种，正序和反序
/// \param delay_ms 每一声响铃之间的延时时间
void BUZZER_beep(int beep_times, int Tone_Mode, int delay_ms) {
    if (beep_times >= 3) {
        beep_times = 3;
    }
    for (int i = 0; i < beep_times; i++) {
        ledcWriteTone(BUZZER_channel, frequency_list[Tone_Mode][i]);
        delay(delay_ms);
        ledcWriteTone(BUZZER_channel, 0);
        delay(delay_ms);
    }
}

////////////////////////////蜂鸣器响函数////////////////////////////
////////////////////////////WS28128与LED同时呼吸////////////////////////////
const int ledc_channel = 0;     // 通道
void breath_LED_setup() {
    /* 使用ledc通道0，设置频率为1kHz，分辨率为10位 */
    int freq = 1000;     // 频率
    int resolution = 8; // 分辨率
    ledcSetup(ledc_channel, freq, resolution); // 设置通道
}

//内置的LED的初始化
void LED_BUILTIN_setup() {
    pinMode(LED_BUILTIN, OUTPUT);
}

#define NUM_LEDS 1 // LED灯珠数量
#define WS2812_DATA_PIN 21 // WS2812信号引脚 长点阵控制引脚
#define LED_TYPE WS2812B // LED灯带型号
#define COLOR_ORDER GRB  // RGB灯珠中红色、绿色、蓝色LED的排列顺序
uint8_t max_bright = 255; // LED亮度控制变量，可使用数值为 0 ～ 255， 数值越大则光带亮度越高
CRGB WS2812B_DZ[NUM_LEDS];  // 建立点阵leds
//初始化WS2812的控制参数
void WS2812_setup() {
    LEDS.addLeds<LED_TYPE, WS2812_DATA_PIN, COLOR_ORDER>(WS2812B_DZ, NUM_LEDS); // 初始化点阵
    FastLED.setBrightness(max_bright);                            // 设置灯带亮度
}

int count = 1;

/// 使LED和WS2812都呼吸
/// \param color 传入需要呼吸灯显示的颜色
/// \param breath_interval_ms 呼吸的时间间隔
void ALL_breath(const struct CRGB &color, int breath_interval_ms) {
    ledcAttachPin(LED_BUILTIN, ledc_channel); // 将通道与对应的引脚连接
    EVERY_N_MILLISECONDS(breath_interval_ms)
    {
        fill_solid(WS2812B_DZ, NUM_LEDS, color);
        max_bright += count;
        if (max_bright >= 255) {
            count = -1;
        } else if (max_bright <= 0) {
            count = 1;
        }
        FastLED.setBrightness(max_bright);
        ledcWrite(ledc_channel, max_bright); // 输出PWM
        FastLED.show();
    }
}

bool state = true;

/// 将WS2812和LED同时闪烁
/// \param color 灯珠的颜色
/// \param blink_interval_ms 闪烁间隔的时间
void ALL_blink(const struct CRGB &color, int blink_interval_ms) {
    EVERY_N_MILLISECONDS(blink_interval_ms)
    {
        if (state) {
            // fill_sold section全部点亮/熄灭，没有时间差
            fill_solid(WS2812B_DZ, NUM_LEDS, color);
            FastLED.show();
        } else {
            fill_solid(WS2812B_DZ, NUM_LEDS, CRGB::Black);
            FastLED.show();
        }
        digitalWrite(LED_BUILTIN, state);
        state = !state;
    }
}

////////////////////////////WS28128与LED同时呼吸////////////////////////////
////////////////////////////热敏电阻温度测量////////////////////////////
#define NTC_INPUT 36//热敏电阻测量引脚

//创建了一个存储所有的温度与热敏电阻值相对应的数组，通过查表法来查找当前的温度
const double temperature_list[] = {32.613, 30.996, 29.468, 28.024, 26.660, 25.370, 24.149, 22.994, 21.901, 20.867,
                                   19.888, 18.959, 18.079, 17.245, 16.454, 15.704, 14.993, 14.317, 13.676, 13.067,
                                   12.489, 11.940, 11.417, 10.921, 10.449, 10.000, 9.572, 9.165, 8.778, 8.409, 8.058,
                                   7.724, 7.405, 7.101, 6.811, 6.534, 6.271, 6.019, 5.779, 5.549, 5.330, 5.121, 4.921,
                                   4.730, 4.547, 4.373, 4.206, 4.046, 3.893, 3.747, 3.607, 3.473, 3.345, 3.222, 3.104,
                                   2.991, 2.883, 2.780, 2.680, 2.585, 2.493, 2.405, 2.321, 2.240, 2.163, 2.088, 2.017,
                                   1.948, 1.882, 1.818, 1.757, 1.698, 1.642, 1.588, 1.535, 1.485, 1.437, 1.390, 1.345,
                                   1.302, 1.261, 1.221, 1.182, 1.145, 1.109, 1.075, 1.041, 1.009, 0.978, 0.948, 0.920,
                                   0.892, 0.865, 0.839, 0., 0.790, 0.767, 0.744, 0.722, 0.701, 0.681, 0.662, 0.643,
                                   0.624, 0.607, 0.589, 0.573, 0.557, 0.541, 0.526, 0.512, 0.498, 0.484, 0.471, 0.458,
                                   0.446, 0.434, 0.422, 0.411, 0.400, 0.390, 0.379, 0.369, 0.360, 0.351, 0.342};


/// 通过传入的测量得到的NTC热敏电阻电阻值查表得到当前的板上温度
/// \param resistance 计算得出的热敏电阻当前的电阻值
/// \return 计算得出的温度
double get_resistance_temperature(double resistance) {
    int i = 0;
    while (i <= 125 && resistance < temperature_list[i]) {
        i++;
    }
    double temperature =
            i - 1 + (temperature_list[i - 1] - resistance) / (temperature_list[i - 1] - temperature_list[i]);
    return temperature;
}

/// 测量热敏电阻的分压，获取当前板子的温度值 bias_temperature偏置温度，他测量的温度不是很准所以要减去一个偏执温度会准确一些
/// \param bias_temperature 输入的偏置温度，最后会减去这个值
/// \return 计算得出的热敏电阻测得的温度
double get_NTC_temperature(double bias_temperature) {
    pinMode(NTC_INPUT, INPUT);
    double NTC_Voltage = (analogRead(NTC_INPUT) / 4096.0) * 3.3;//这里的4096一定要写成小数的形式，或者在式子前面乘以1.0也行，因为两个整数相除，结果必定是整数
//    Serial.print("NTC_Voltage");
//    Serial.println(NTC_Voltage);
    double NTC_resistance = (10 * NTC_Voltage) / (5 - NTC_Voltage);
    double temperature = get_resistance_temperature(NTC_resistance) - bias_temperature;
    return temperature;
}
////////////////////////////热敏电阻温度测量////////////////////////////
////////////////////////////获取拨码开关输入值////////////////////////////
#define IN0 39
#define IN1 34
#define IN2 35

/// 获取拨码开关的输入，将其转化为整型
/// \return 计算得出的拨码开关上面的数值
int get_DIP_switch_number() {
    bool x0, x1, x2;
    pinMode(IN0, INPUT);
    pinMode(IN1, INPUT);
    pinMode(IN2, INPUT);
    x0 = digitalRead(IN0);
    x1 = digitalRead(IN1);
    x2 = digitalRead(IN2);
    int result = x0 * 1 + x1 * 2 + x2 * 4;
    return result;
}

////////////////////////////获取拨码开关输入值////////////////////////////
// Required to replace with your xbox address
XboxSeriesXControllerESP32_asukiaaa::Core
        xboxController("98:7a:14:a4:65:f1");
float joyLHorizon, joyLVertical, joyRHorizon, joyRVertical, trigLT, trigRT;
bool btnA, btnB, btnX, btnY;
bool btnShare, btnStart, btnSelect, btnXbox;
bool btnLB, btnRB, btnLS, btnRS;
bool btnDirUp, btnDirLeft, btnDirRight, btnDirDown;
float joystickMax = XboxControllerNotificationParser::maxJoy;

///用于解析Xbox手柄的要输入，将他们转化成0-1之前的小数
void xboxController_parse() {
    joyLHorizon = (float) (xboxController.xboxNotif.joyLHori);
    joyLVertical = (float) (xboxController.xboxNotif.joyLVert);
    joyRHorizon = (float) (xboxController.xboxNotif.joyRHori);
    joyRVertical = (float) (xboxController.xboxNotif.joyRVert);
    trigLT = (float) (xboxController.xboxNotif.trigLT);
    trigRT = (float) (xboxController.xboxNotif.trigRT);
    btnA = xboxController.xboxNotif.btnA;
    btnB = xboxController.xboxNotif.btnB;
    btnX = xboxController.xboxNotif.btnX;
    btnY = xboxController.xboxNotif.btnY;
    btnShare = xboxController.xboxNotif.btnShare;//我这个手柄没有这个键貌似是最新版手柄的
    btnStart = xboxController.xboxNotif.btnStart;
    btnSelect = xboxController.xboxNotif.btnSelect;
    btnXbox = xboxController.xboxNotif.btnXbox;
    btnLB = xboxController.xboxNotif.btnLB;
    btnRB = xboxController.xboxNotif.btnRB;
    btnLS = xboxController.xboxNotif.btnLS;//左摇杆按下键
    btnRS = xboxController.xboxNotif.btnRS;//右摇杆按下键
    btnDirUp = xboxController.xboxNotif.btnDirUp;
    btnDirLeft = xboxController.xboxNotif.btnDirLeft;
    btnDirRight = xboxController.xboxNotif.btnDirRight;
    btnDirDown = xboxController.xboxNotif.btnDirDown;
}

void setup() {
    // 编码器设置
    // monitor接口设置
    Serial.begin(115200);
    I2C_one.begin(17, 5, 400000UL);
    I2C_two.begin(18, 19, 400000UL);
    sensor0.init(&I2C_one);
    sensor1.init(&I2C_two);
    mpu6050.begin();
    mpu6050.calcGyroOffsets(true);
    motor0.linkSensor(&sensor0);
    motor1.linkSensor(&sensor1);
    // 驱动器设置
    driver0.voltage_power_supply = 12;
    driver0.init();
    motor0.linkDriver(&driver0);
    driver1.voltage_power_supply = 12;
    driver1.init();
    motor1.linkDriver(&driver1);
    // 电流限制
    motor0.current_limit = 5;
    motor1.current_limit = 5;
    // 电压限制
    motor0.voltage_limit = 12;
    motor1.voltage_limit = 12;
    motor0.velocity_limit = 1000;
    motor1.velocity_limit = 1000;
    // 电流检测
    current_sense0.init();
    current_sense0.gain_b *= -1;
//    current_sense1.skip_align = true; //这里可以先设置为False看看效果
    current_sense0.skip_align = true; //这里可以先设置为False看看效果
    motor0.linkCurrentSense(&current_sense0);
    // current sense init and linking
    current_sense1.init();
    current_sense1.gain_b *= -1;
//    current_sense1.skip_align = true;
    current_sense1.skip_align = true;
    motor1.linkCurrentSense(&current_sense1);
    // 控制环
    // 其他模式 TorqueControlType::voltage TorqueControlType::dc_current
    motor0.torque_controller = TorqueControlType::voltage;
    motor0.foc_modulation = FOCModulationType::SpaceVectorPWM;
    motor0.controller = MotionControlType::torque;
    motor1.torque_controller = TorqueControlType::voltage;
    motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
    motor1.controller = MotionControlType::torque;
    // FOC电流控制PID参数
    // current q loop PID
    motor0.PID_current_q.P = 20.0;
    motor0.PID_current_q.I = 0.0;
    motor0.PID_current_q.D = 0.0;
    motor0.PID_current_q.output_ramp = 0.0;
    motor0.PID_current_q.limit = 12.0;
// Low pass filtering time constant
    motor0.LPF_current_q.Tf = 0.02;
// current d loop PID
    motor0.PID_current_d.P = 20.0;
    motor0.PID_current_d.I = 0.0;
    motor0.PID_current_d.D = 0.0;
    motor0.PID_current_d.output_ramp = 0.0;
    motor0.PID_current_d.limit = 12.0;
// Low pass filtering time constant
    motor0.LPF_current_d.Tf = 0.02;
    // current q loop PID
    motor1.PID_current_q.P = 20.0;
    motor1.PID_current_q.I = 0.0;
    motor1.PID_current_q.D = 0.0;
    motor1.PID_current_q.output_ramp = 0.0;
    motor1.PID_current_q.limit = 12.0;
// Low pass filtering time constant
    motor1.LPF_current_q.Tf = 0.02;
// current d loop PID
    motor1.PID_current_d.P = 20.0;
    motor1.PID_current_d.I = 0.0;
    motor1.PID_current_d.D = 0.0;
    motor1.PID_current_d.output_ramp = 0.0;
    motor1.PID_current_d.limit = 12.0;
// Low pass filtering time constant
    motor1.LPF_current_d.Tf = 0.02;
    // 速度环PID参数 这个代码是使用力矩控制模式的，所以即使设置了这个速度环的PID也没有用不会起作用
//    而且我调试了一下发现在FocCurrent模式下速度环效果并不好，可能是我没有调好的原因，但是在平衡车的控制中我们用的力矩模式，所以没啥用
    // velocity loop PID
    motor0.PID_velocity.P = 0.05;
    motor0.PID_velocity.I = 0.0;
    motor0.PID_velocity.D = 0.0;
    motor0.PID_velocity.output_ramp = 0.0;
    motor0.PID_velocity.limit = 100.0;
// Low pass filtering time constant
    motor0.LPF_velocity.Tf = 0.01;
    // angle loop PID
    motor0.P_angle.P = 70.0;
    motor0.P_angle.I = 0.5;
    motor0.P_angle.D = 0.0;
    motor0.P_angle.output_ramp = 0.0;
    motor0.P_angle.limit = 100.0;
// Low pass filtering time constant
    motor0.LPF_angle.Tf = 0.0;
    // 速度环PID参数
    // velocity loop PID
    motor1.PID_velocity.P = 0.05;
    motor1.PID_velocity.I = 0.0;
    motor1.PID_velocity.D = 0.0;
    motor1.PID_velocity.output_ramp = 0.0;
    motor1.PID_velocity.limit = 100.0;
// Low pass filtering time constant
    motor1.LPF_velocity.Tf = 0.01;
    // angle loop PID
    motor1.P_angle.P = 70.0;
    motor1.P_angle.I = 0.5;
    motor1.P_angle.D = 0.0;
    motor1.P_angle.output_ramp = 0.0;
    motor1.P_angle.limit = 100.0;
// Low pass filtering time constant
    motor1.LPF_angle.Tf = 0.0;
    // comment out if not needed
//    motor0.useMonitoring(Serial);
//    motor1.useMonitoring(Serial);
//    monitor相关设置
//    motor0.monitor_downsample = 0;
//    motor0.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
//    motor1.monitor_downsample = 0;
//    motor1.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
    //电机初始化
    motor0.init();
    // align encoder and start FOC
//    motor0.initFOC(5.41, CCW);
    motor0.initFOC();
    motor1.init();
    // align encoder and start FOC
//    motor1.initFOC(3.55, CW);
    motor1.initFOC();
    // 初始目标值
    motor0.target = 0.00;
    motor1.target = 0.00;
    // 映射电机到commander
    commander.add('S', cntStab, "pid stab");
    commander.add('V', cntMove, "pid vel");
    commander.add('P', lpfThrottle, "lpf vel commander");
    commander.add('T', lpfPitch, "lpf throttle");
    commander.add('E', lpfSteering, "lpf steering");
    Serial.println(F("Double motor sketch ready."));
    BUZZER_setup();
    xboxController.begin();
    BUZZER_beep(3, 0, 100);
    breath_LED_setup();
    LED_BUILTIN_setup();
    WS2812_setup();
}

int commaPosition = 0; //存储还没有分离出来的字符串
String serialReceiveUserCommand() {
    // a string to hold incoming data
    static String received_chars;
    String command = "";
    while (Serial.available()) {
        // get the new byte:
        char inChar = (char) Serial.read();
        // add it to the string buffer:
        received_chars += inChar;
        // end of user input
        if (inChar == '\n') {
            Serial.println(command);
            // execute the user commander
            command = received_chars;
            //根据逗号分离两组字符串
            //想象中双足轮通讯数据格式:高度,指令   指令格式：英文+值
            commaPosition = command.indexOf('G'); //检测字符串中的逗号
            if (commaPosition != -1)              //如果有逗号存在就向下执行
            {
                //一号电机角度
                Offset_parameters = command.substring(commaPosition + 1, command.length()).toFloat(); //打印字符串，从当前位置+1开始
                Serial.println(Offset_parameters);
            }
            // reset the commander buffer
            received_chars = "";
        }
    }
    return command;
}

bool btnLB_press_flag = false; //避免XBOX肩键按下的时候重复加很多次数
bool btnRB_press_flag = false; //避免XBOX肩键按下的时候重复加很多次数
void loop() {
    xboxController.onLoop();
    ALL_breath(CRGB::Green, 1);
    if (xboxController.isConnected()) {
        xboxController_parse();
        throttle = joyLVertical * 50 / joystickMax - 25;
        if (throttle >= max_throttle) {
            throttle = max_throttle;
        }
        steering = joyRHorizon * 8 / joystickMax - 4;
        if (btnLB) {
            btnLB_press_flag = true;
        }
        if (!btnLB && btnLB_press_flag) {
            Offset_parameters += 0.1;
            btnLB_press_flag = false;
        }
        if (btnRB) {
            btnRB_press_flag = true;
        }
        if (!btnRB && btnRB_press_flag) {
            Offset_parameters -= 0.1;
            btnRB_press_flag = false;
        }
//        Serial.println(Offset_parameters);
        // iterative setting FOC phase voltage
        //motor0是右轮 如果开关是机器人的前面的话 motor1是左轮
        motor0.loopFOC();
        motor1.loopFOC();
        // iterative function setting the outer loop target
        motor0.move();
        motor1.move();
        mpu6050.update();
        float mpu_pitch = mpu6050.getAngleX();// tockn的get_angle，通过一阶置信计算
        float right_left_average_velocity = (motor0.shaft_velocity + motor1.shaft_velocity) / 2;
        //速度环计算用户输入的油门和当前车辆的速度，得出需要车辆产生的倾角，输出传给直立环产生相应的角度
        float target_pitch = lpf_pitch_cmd(pid_vel(right_left_average_velocity - lpf_throttle(throttle)));
        float voltage_control = pid_stb(Offset_parameters - mpu_pitch + target_pitch);
        //通过低通滤波器得到用户输入的预期的转向量
        float steering_adj = lpf_steering(steering);
//        float right_left_wheel_difference = motor0.shaft_velocity - motor1.shaft_velocity;
//        float turn_adj = pid_steer(right_left_wheel_difference);
        float right_wheel_difference = motor0.shaft_velocity - right_left_average_velocity;
        float left_wheel_difference = motor1.shaft_velocity - right_left_average_velocity;
        float right_turn_adj = pid_right_steer(abs(right_wheel_difference) - steering_adj);
        float left_turn_adj = pid_left_steer(abs(left_wheel_difference) - steering_adj);
        motor0.target = -(voltage_control + steering_adj);
        motor1.target = -(voltage_control - steering_adj);
    } else {
        Serial.print("not connected");
        if (xboxController.getCountFailedConnection() > 2) {
            ESP.restart();
        }
    }
}
